#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
/*
    发布方：订阅乌龟的位姿信息，转换为相对于窗体的坐标关系，并发布
    准备：
        查看话题（rostopic lis）: /turtle1/pose
        查看消息类型（rostopic info）：turtlesim/Pose
        查看消息格式（rosmsg info）：
                                float32 x
                                float32 y
                                float32 theta
                                float32 linear_velocity
                                float32 angular_velocity
*/
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    // 获取位姿信息，转换成坐标系相对关系（核心），并发布
    //创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";   // 世界坐标系名称
    ts.header.stamp = ros::Time::now(); // 时间
    ts.child_frame_id = "turtle1";      // 乌龟名称
    //坐标偏移量
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0.0;   // 二维实现，pose没有z，z为0
    //四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "demo_1");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/turtle1/pose", 100, doPose);

    ros::spin();
    return 0;
}
